/*
 * topological-hex, a program to compute combinatorial hexahedral meshes.
 *
 * Copyright (C) <2018> <Université catholique de Louvain (UCL), Belgique>
 *
 * List of the contributors to the development, description and complete
 * License: see LICENSE file.
 *
 * This program (topological-hex) is free software:
 * you can redistribute it and/or modify it under the terms
 * of the GNU General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program (see COPYING file).  If not,
 * see <http://www.gnu.org/licenses/>.
 */


#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <ctype.h>

#include "solver.h"
#include "symmetry.h"

static void build_initial_adjacency(solver *solver, uint32_t num_boundary_quads);

error_code solver_init(solver *solver, mesh *mesh, uint32_t num_vertices,
                       uint8_t flags) {
  solver->hex_id = (hex_index)-1;
  solver->bound_mask = ~(uint32_t)0;

  solver->num_vertices = num_vertices;
  solver->num_quads    = mesh->num_quads;
  solver->num_hexes    = mesh->num_hexes;
  solver->num_branches = 0;

  solver->flags = flags;

  solver->max_quad_id   = mesh->num_boundary_quads;
  solver->max_vertex_id = mesh_num_boundary_vertices(mesh);

  solver->quads         = mesh->quads;
  solver->hexes         = mesh->hexes;
  solver->hex_adjacency = mesh->hex_adjacency;
  solver->hex_facets    = mesh->hex_facets;

  solver->ub_data = NULL;

  if (solver->flags & SOLVER_SHELLABLE_ONLY) {
    if (posix_memalign((void**)&solver->quad_adjacency, ALIGNMENT,
                       4 * sizeof(*solver->quad_adjacency) * mesh->num_quads) != 0) {
      goto fail_alloc_quad_adjacency;
    }

    build_initial_adjacency(solver, mesh->num_boundary_quads);
  }
  else
    solver->quad_adjacency = NULL;

  solver->quad_queue_size   = mesh->num_boundary_quads;
  solver->quad_queue_offset = 0;
  if (posix_memalign((void**)&solver->quad_queue, ALIGNMENT,
                     sizeof(*solver->quad_queue)*mesh->num_quads) != 0) {
    goto fail_alloc_queue;
  }

  for (uint32_t i = 0; i < mesh->num_boundary_quads; i++)
    solver->quad_queue[i] = i;

  if (posix_memalign((void**)&solver->num_occurrences, ALIGNMENT,
                     sizeof(*solver->num_occurrences)*mesh->num_quads) != 0) {
    goto fail_alloc_num_occurrences;
  }

  for (uint32_t i = 0; i < mesh->num_boundary_quads; i++)
    solver->num_occurrences[i] = 1;
  for (uint32_t i = mesh->num_boundary_quads; i < mesh->num_quads; i++)
    solver->num_occurrences[i] = 2;

  if (posix_memalign(
        (void**)&solver->quad_descriptors, ALIGNMENT,
        sizeof(*solver->quad_descriptors)*
        num_vertices * num_vertices)) {
    goto fail_alloc_quad_descriptors;
  }

  for (uint32_t i = 0; i < num_vertices * num_vertices; i++) {
    solver->quad_descriptors[i] = EMPTY_QUAD_DESCRIPTOR;
  }

  for (quad_index i = 0; i < mesh->num_boundary_quads; i++)  {
    vertex_index a = mesh->quads[4*i + 0], b = mesh->quads[4*i + 1],
      c = mesh->quads[4*i + 2], d = mesh->quads[4*i + 3];
    solver->quad_descriptors[diagonal_index(num_vertices, a, c)] =
      make_quad_descriptor(i, NO_HEXAHEDRON, b, d);
    solver->quad_descriptors[diagonal_index(num_vertices, b, d)] =
      make_quad_descriptor(i, NO_HEXAHEDRON, a, c);
  }

  if (posix_memalign(
        (void**)&solver->allowed_neighbors, ALIGNMENT,
        sizeof(*solver->allowed_neighbors)*num_vertices) != 0) {
    goto fail_alloc_allowed_neighbors;
  }

  if (posix_memalign(
        (void**)&solver->known_neighbors, ALIGNMENT,
        sizeof(*solver->known_neighbors)*num_vertices) != 0) {
    goto fail_alloc_known_neighbors;
  }

  if (posix_memalign(
        (void**)&solver->known_diagonals, ALIGNMENT,
        sizeof(*solver->known_diagonals)*num_vertices) != 0) {
    goto fail_alloc_known_diagonals;
  }

  if (posix_memalign(
        (void**)&solver->known_quad_diagonals, ALIGNMENT,
        sizeof(*solver->known_quad_diagonals)*num_vertices) != 0) {
    goto fail_alloc_known_quad_diagonals;
  }

  for (vertex_index i = 0; i < num_vertices; i++) {
    vertex_set_init_up_to(solver->allowed_neighbors + i, num_vertices);
    vertex_set_remove(solver->allowed_neighbors + i, i);
  }

  for (vertex_index i = 0; i < num_vertices; i++)
    vertex_set_init_empty(solver->known_neighbors + i);

  for (vertex_index i = 0; i < num_vertices; i++)
    vertex_set_init_empty(solver->known_diagonals + i);

  for (vertex_index i = 0; i < num_vertices; i++)
    vertex_set_init_empty(solver->known_quad_diagonals + i);

  vertex_set_init_empty(&solver->finished_vertices);

  vertex_set_init_empty(&solver->even_vertices);
  vertex_set_init_empty(&solver->odd_vertices);

  for (quad_vertex_index i = 0; i < 4; i++) {
    if (i % 2 == 0)
      vertex_set_insert(&solver->even_vertices, solver->quads[i]);
    else
      vertex_set_insert(&solver->odd_vertices, solver->quads[i]);
  }

  for (quad_index i = 0; i < mesh->num_boundary_quads; i++) {
    vertex_index quad[4];
    memcpy(quad, mesh->quads + 4*i, sizeof(quad));

    for (quad_vertex_index j = 0; j < 4; j++) {
      vertex_index v = quad[0];
      vertex_set_insert(solver->known_neighbors + v, quad[1]);
      vertex_set_insert(solver->known_neighbors + v, quad[3]);
      vertex_set_remove(solver->allowed_neighbors + v, quad[2]);
      vertex_set_insert(solver->known_quad_diagonals + v, quad[2]);

      /*
       * This assumes consecutive quadrangles share vertices. This is the case
       * because of the reordering of quadrangles when computing a consistent
       * orientation for quadrangles.
       */
      if (!vertex_set_contains(&solver->even_vertices, v) &&
          !vertex_set_contains(&solver->odd_vertices, v)) {
        if (vertex_set_contains(&solver->even_vertices, quad[1]) ||
            vertex_set_contains(&solver->even_vertices, quad[3]) ||
            vertex_set_contains(&solver->odd_vertices, quad[2]))
          vertex_set_insert(&solver->odd_vertices, v);
        else
          vertex_set_insert(&solver->even_vertices, v);
      }

      memmove(quad, quad+1, 3*sizeof(*quad));
      quad[3] = v;
    }
  }

  solver->no_goods = NULL;
  solver->no_goods_size     = 0;
  solver->no_goods_capacity = 0;

  solver->boundary_group = NULL;
  solver->num_symmetries = 0;
  solver->num_boundary_vertices = 0;

  solver->symmetry_data = NULL;

  return SUCCESS;

  free(solver->known_quad_diagonals);
fail_alloc_known_quad_diagonals:
  free(solver->known_diagonals);
fail_alloc_known_diagonals:
  free(solver->known_neighbors);
fail_alloc_known_neighbors:
  free(solver->allowed_neighbors);
fail_alloc_allowed_neighbors:
  free(solver->quad_descriptors);
fail_alloc_quad_descriptors:
  free(solver->num_occurrences);
fail_alloc_num_occurrences:
  free(solver->quad_queue);
fail_alloc_queue:
  if (solver->flags & SOLVER_SHELLABLE_ONLY)
    free(solver->quad_adjacency);
fail_alloc_quad_adjacency:
  return OUT_OF_MEMORY;
}

void solver_release(solver *solver) {
  free(solver->symmetry_data);
  free(solver->boundary_group);
  free(solver->no_goods);
  free(solver->known_quad_diagonals);
  free(solver->known_diagonals);
  free(solver->known_neighbors);
  free(solver->allowed_neighbors);
  free(solver->quad_descriptors);
  free(solver->num_occurrences);
  free(solver->quad_queue);
  free(solver->quad_adjacency);
}

error_code solver_init_copy(struct solver *solver, const struct solver *other) {
  if (posix_memalign((void**)&solver->quads, ALIGNMENT,
                     other->num_quads * 4 * sizeof(*solver->quads)) != 0) {
    goto fail_alloc_quads;
  }

  if (posix_memalign((void**)&solver->hexes, ALIGNMENT,
                     other->num_hexes * 8 * sizeof(*solver->hexes)) != 0) {
    goto fail_alloc_hexes;
  }

  if (posix_memalign((void**)&solver->hex_adjacency, ALIGNMENT,
                     6 * other->num_hexes *
                     sizeof(*solver->hex_adjacency)) != 0) {
    goto fail_alloc_hex_adjacency;
  }

  if (posix_memalign((void**)&solver->hex_facets, ALIGNMENT,
                     6 * other->num_hexes *
                     sizeof(*solver->hex_facets)) != 0) {
    goto fail_alloc_hex_facets;
  }

  if (posix_memalign((void**)&solver->quad_adjacency, ALIGNMENT,
                     4 * other->num_quads * sizeof(*solver->quad_adjacency)) != 0) {
    goto fail_alloc_quad_adjacency;
  }

  if (posix_memalign((void**)&solver->quad_queue, ALIGNMENT,
                     other->num_quads * sizeof(*solver->quad_queue)) != 0) {
    goto fail_alloc_quad_queue;
  }

  if (posix_memalign((void**)&solver->num_occurrences, ALIGNMENT,
                     other->num_quads *
                     sizeof(*solver->num_occurrences)) != 0) {
    goto fail_alloc_num_occurrences;
  }

  if (posix_memalign((void**)&solver->quad_descriptors, ALIGNMENT,
                     other->num_vertices * other->num_vertices *
                     sizeof(*solver->quad_descriptors)) != 0) {
    goto fail_alloc_quad_descriptors;
  }

  if (posix_memalign((void**)&solver->allowed_neighbors, ALIGNMENT,
                     other->num_vertices *
                     sizeof(*solver->allowed_neighbors)) != 0) {
    goto fail_alloc_allowed_neighbors;
  }

  if (posix_memalign((void**)&solver->known_neighbors, ALIGNMENT,
                     other->num_vertices *
                     sizeof(*solver->known_neighbors)) != 0) {
    goto fail_alloc_known_neighbors;
  }

  if (posix_memalign((void**)&solver->known_diagonals, ALIGNMENT,
                     other->num_vertices *
                     sizeof(*solver->known_diagonals)) != 0) {
    goto fail_alloc_known_diagonals;
  }

  if (posix_memalign((void**)&solver->known_quad_diagonals, ALIGNMENT,
                     other->num_vertices *
                     sizeof(*solver->known_quad_diagonals)) != 0) {
    goto fail_alloc_known_quad_diagonals;
  }

  solver->ub_data = NULL;

  solver->symmetry_data = NULL;

  if (other->num_symmetries == 0) {
    solver->boundary_group = NULL;
    solver->num_symmetries        = 0;
    solver->num_boundary_vertices = 0;
  }
  else {
    if (posix_memalign((void**)&solver->boundary_group, ALIGNMENT,
                       sizeof(*solver->boundary_group) *
                       other->num_boundary_vertices *
                       other->num_symmetries) != 0)
      goto fail_alloc_boundary_group;

    solver->num_symmetries = other->num_symmetries;
    solver->num_boundary_vertices = other->num_boundary_vertices;

    if (solver->flags & SOLVER_SHELLABLE_ONLY) {
      if (posix_memalign((void**)&solver->symmetry_data, ALIGNMENT,
                         solver_incremental_symmetry_data_size(
                           other->num_vertices, other->num_hexes, other->num_symmetries)) != 0)
        goto fail_alloc_symmetry_data;
    }
  }

  solver->no_goods = NULL;
  solver->no_goods_size     = 0;
  solver->no_goods_capacity = 0;

  solver_copy(solver, other);

  return SUCCESS;

  free(solver->symmetry_data);
fail_alloc_symmetry_data:
  free(solver->boundary_group);
fail_alloc_boundary_group:
  free(solver->known_quad_diagonals);
fail_alloc_known_quad_diagonals:
  free(solver->known_diagonals);
fail_alloc_known_diagonals:
  free(solver->known_neighbors);
fail_alloc_known_neighbors:
  free(solver->allowed_neighbors);
fail_alloc_allowed_neighbors:
  free(solver->quad_descriptors);
fail_alloc_quad_descriptors:
  free(solver->num_occurrences);
fail_alloc_num_occurrences:
  free(solver->quad_queue);
fail_alloc_quad_queue:
  free(solver->quad_adjacency);
fail_alloc_quad_adjacency:
  free(solver->hex_facets);
fail_alloc_hex_facets:
  free(solver->hex_adjacency);
fail_alloc_hex_adjacency:
  free(solver->hexes);
fail_alloc_hexes:
  free(solver->quads);
fail_alloc_quads:
  return OUT_OF_MEMORY;
}

void solver_release_copy(solver *solver) {
  free(solver->symmetry_data);
  free(solver->boundary_group);
  free(solver->no_goods);
  free(solver->known_quad_diagonals);
  free(solver->known_diagonals);
  free(solver->known_neighbors);
  free(solver->allowed_neighbors);
  free(solver->quad_descriptors);
  free(solver->num_occurrences);
  free(solver->quad_queue);
  free(solver->quad_adjacency);
  free(solver->hex_facets);
  free(solver->hex_adjacency);
  free(solver->hexes);
  free(solver->quads);
}

void solver_copy(struct solver *solver, const struct solver *other) {
  solver->num_vertices = other->num_vertices;
  solver->num_quads    = other->num_quads;
  solver->num_hexes    = other->num_hexes;
  solver->num_branches = other->num_branches;

  solver->flags = other->flags;

  solver->hex_id = other->hex_id;

  solver->max_quad_id   = other->max_quad_id;
  solver->max_vertex_id = other->max_vertex_id;

  solver->quad_queue_size   = other->quad_queue_size;
  solver->quad_queue_offset = other->quad_queue_offset;

  solver->bound_mask = other->bound_mask;

  solver->cb        = other->cb;
  solver->user_data = other->user_data;

  memcpy(solver->domains, other->domains, sizeof(solver->domains));
  solver->bound_mask = other->bound_mask;

  memcpy(solver->quads, other->quads,
         solver->num_quads * 4 * sizeof(*solver->quads));

  memcpy(solver->hexes, other->hexes,
         solver->num_hexes * 8 * sizeof(*solver->hexes));

  memcpy(solver->hex_adjacency, other->hex_adjacency,
         6 * solver->num_hexes * sizeof(*solver->hex_adjacency));

  memcpy(solver->hex_facets, other->hex_facets,
         6 * solver->num_hexes * sizeof(*solver->hex_facets));

  if (other->flags & SOLVER_SHELLABLE_ONLY) {
    memcpy(solver->quad_adjacency, other->quad_adjacency,
           4 * solver->num_quads * sizeof(*solver->quad_adjacency));
    if (other->symmetry_data)
      memcpy(solver->symmetry_data, other->symmetry_data,
             solver_incremental_symmetry_data_size(
               other->num_vertices, other->num_hexes,
               other->num_symmetries));
  }

  memcpy(solver->quad_queue, other->quad_queue,
         solver->num_quads * sizeof(*solver->quad_queue));

  memcpy(solver->num_occurrences, other->num_occurrences,
         solver->num_quads * sizeof(*solver->num_occurrences));

  memcpy(solver->quad_descriptors, other->quad_descriptors,
         solver->num_vertices * solver->num_vertices *
         sizeof(*solver->quad_descriptors));

  memcpy(solver->allowed_neighbors, other->allowed_neighbors,
         solver->num_vertices * sizeof(*solver->allowed_neighbors));

  memcpy(solver->known_neighbors, other->known_neighbors,
         solver->num_vertices * sizeof(*solver->known_neighbors));

  memcpy(solver->known_diagonals, other->known_diagonals,
         solver->num_vertices * sizeof(*solver->known_diagonals));

  memcpy(solver->known_quad_diagonals, other->known_quad_diagonals,
         solver->num_vertices * sizeof(*solver->known_quad_diagonals));

  solver->finished_vertices = other->finished_vertices;
  solver->even_vertices = other->even_vertices;
  solver->odd_vertices = other->odd_vertices;

  if (solver->no_goods) {
    free(solver->no_goods);

    solver->no_goods = NULL;
    solver->no_goods_size     = 0;
    solver->no_goods_capacity = 0;
  }

  if (other->no_goods_size != 0) {
    /*
     * We only allocate enough storage for the no goods of the original object,
     * which means the capacity of the copy can be smaller.
     */
    if (posix_memalign((void**)&solver->no_goods, ALIGNMENT,
                       sizeof(*solver->no_goods) * other->no_goods_size) == 0) {
      memcpy(solver->no_goods, other->no_goods,
             sizeof(*solver->no_goods) * other->no_goods_size);

      solver->no_goods_size = other->no_goods_size;
      solver->no_goods_capacity = other->no_goods_size;
    }
  }

  if (other->num_symmetries != 0) {
    memcpy(solver->boundary_group, other->boundary_group,
           sizeof(*solver->boundary_group) *
           other->num_boundary_vertices * other->num_symmetries);
    solver->num_symmetries = other->num_symmetries;
    solver->num_boundary_vertices = other->num_boundary_vertices;
  }
}

static quad_edge find_edge(const solver *solver, uint32_t num_boundary_quads,
                           const vertex_index *edge) {
  for (quad_index i_b = 0; i_b < num_boundary_quads; i_b++) {
    for (quad_edge_index j_b = 0; j_b < 4; j_b++) {
      vertex_index other_edge[2] = {solver->quads[4 * i_b + j_b],
                                    solver->quads[4 * i_b + ((j_b + 1) % 4)]};
      if (edge[0] == other_edge[1] && edge[1] == other_edge[0])
        return make_quad_edge(i_b, j_b);
    }
  }

  return make_quad_edge(NO_QUAD, 0);
}

static void build_initial_adjacency(solver *solver, uint32_t num_boundary_quads) {
  for (quad_index i_a = 0; i_a < num_boundary_quads; i_a++)  {
    for (quad_edge_index j_a = 0; j_a < 4; j_a++) {
      vertex_index edge_a[2] = {solver->quads[4 * i_a + j_a],
                                solver->quads[4 * i_a + ((j_a + 1) % 4)]};

      solver->quad_adjacency[4 * i_a + j_a] = find_edge(solver, num_boundary_quads,
                                                        edge_a);
    }
  }
}

void solver_print(solver *solver, uint32_t num_hexes, const char *header) {
  for (size_t i = 0; i < 20; i++) printf("-");
  printf(" %s ", header);
  for (size_t i = 0; i < 80 - strlen(header) - 2 - 20; i++) printf("-");
  printf("\n");
  printf("Completed hexahedra:\n");
  for (hex_index i = 0; i < num_hexes; i++) {
    printf("  %u) [", i);
    for (hex_vertex_index j = 0; j < 8; j++) {
      printf("%u", solver->hexes[8*i+j]);
      if (j + 1 != 8) printf(", ");
    }
    printf("]\n");
  }
  printf("\n");
  printf("Current hexahedron:\n");
  hex_index i = num_hexes;
  printf("  [");
  for (quad_vertex_index j = 0; j < 4; j++) {
    printf("%u, ", solver->hexes[8*i + j]);
  }
  for (quad_vertex_index j = 0; j < 4; j++) {
    printf("\n   {");
    VERTEX_SET_FOREACH(solver->domains[j], x) {
      printf("%u, ", (vertex_index)x);
    }
    printf("}");
    if (j + 1 != 4) printf(",");
    else printf("]");
  }
  printf("\n\n");
  printf("Quads:\n");
  for (quad_index i = 0; i < solver->max_quad_id; i++) {
    printf(" %u) [", i);
    for (quad_vertex_index j = 0; j < 4; j++) {
      printf("%u", solver->quads[4*i + j]);
      if (j+1 != 4) printf(", ");
    }
    printf("]\n");
  }
  printf("\n");
  if (solver->flags & SOLVER_SHELLABLE_ONLY) {
    printf("Quad adjacency:\n");
    for (quad_index i = 0; i < solver->max_quad_id; i++) {
      printf(" %u) [", i);
      for (quad_edge_index j = 0; j < 4; j++) {
        printf("(%u, %u)",
               quad_edge_quad(solver->quad_adjacency[4*i + j]),
               quad_edge_edge(solver->quad_adjacency[4*i + j]));
        if (j+1 != 4) printf(", ");
      }
      printf("]\n");
    }
    printf("\n");
  }
  printf("Quad queue: ");
  for (uint32_t i = solver->quad_queue_offset;
       i < solver->quad_queue_size; i++) {
    if (solver->num_occurrences[solver->quad_queue[i]] == 0)
      printf("(%u) ", solver->quad_queue[i]);
    else
      printf("%u ", solver->quad_queue[i]);
  }
  printf("\n\n");
  printf("Bound mask: ");
  for (uint32_t i = 0; i < 9; i++) {
    printf("%d", ((solver->bound_mask & (1 << i))) ? 1 : 0);
  }
  printf("\n\n");
  printf("Known neighbors:\n");
  for (vertex_index i = 0; i < solver->num_vertices; i++) {
    printf(" %u) {", i);
    VERTEX_SET_FOREACH(solver->known_neighbors[i], v) {
      printf("%u, ", (vertex_index)v);
    }
    printf("}\n");
  }
  printf("\n");
  printf("Allowed neighbors:\n");
  for (vertex_index i = 0; i < solver->num_vertices; i++) {
    printf(" %u) {", i);
    VERTEX_SET_FOREACH(solver->allowed_neighbors[i], v) {
      printf("%u, ", (vertex_index)v);
    }
    printf("}\n");
  }
  for (int i = 0; i < 80; i++) printf("-");
  printf("\n\n");
  fflush(stdout);
}

typedef struct pool_offsets {
  uint32_t quads;

  uint32_t hexes;
  uint32_t hex_adjacency;
  uint32_t hex_facets;
  uint32_t quad_adjacency;

  uint32_t quad_queue;
  uint32_t num_occurrences;

  uint32_t quad_descriptors;

  uint32_t allowed_neighbors;
  uint32_t known_neighbors;
  uint32_t known_diagonals;
  uint32_t known_quad_diagonals;

  uint32_t symmetries;
  uint32_t symmetry_data;

  uint32_t size;
} pool_offsets;

static
void compute_offsets(pool_offsets *offsets,
                     uint32_t num_vertices, uint32_t num_quads,
                     uint32_t num_hexes, uint32_t num_symmetries,
                     uint32_t num_boundary_vertices,
                     uint8_t flags);

static
void apply_offsets(solver *solver, const pool_offsets *offsets);

error_code solver_pool_alloc(solver_pool *pool,
                             uint32_t num_vertices,
                             uint32_t num_quads,
                             uint32_t num_hexes,
                             uint32_t num_symmetries,
                             uint32_t num_boundary_vertices,
                             uint8_t flags,
                             uint32_t n) {
  pool_offsets offsets;
  compute_offsets(&offsets, num_vertices, num_quads, num_hexes,
                  num_symmetries, num_boundary_vertices,
                  flags);

  if (posix_memalign(&pool->data, ALIGNMENT, n * (size_t)offsets.size) != 0)
    return OUT_OF_MEMORY;

  pool->element_size = offsets.size;
  pool->capacity = n;

  for (uint32_t i = 0; i < n; i++) {
    char *base = (char*)pool->data + i * (size_t)offsets.size;

    struct solver *solver = (struct solver*)base;

    apply_offsets(solver, &offsets);

    solver->num_vertices          = num_vertices;
    solver->num_quads             = num_quads;
    solver->num_hexes             = num_hexes;
    solver->num_symmetries        = num_symmetries;
    solver->num_boundary_vertices = num_boundary_vertices;
    solver->flags                 = flags;

    solver->num_branches = 0;

    solver->no_goods = NULL;
    solver->no_goods_size     = 0;
    solver->no_goods_capacity = 0;

    solver->ub_data = NULL;
  }

  return SUCCESS;
}

error_code solver_pool_init(solver_pool *pool, const struct solver *template,
                            uint32_t n) {
  return solver_pool_alloc(
    pool, template->num_vertices, template->num_quads, template->num_hexes,
    template->num_symmetries, template->num_boundary_vertices,
    template->flags,
    n);
}

error_code solver_pool_realloc(solver_pool *pool, uint32_t n) {
  solver *template = solver_pool_get(pool, 0);

  pool_offsets offsets;
  compute_offsets(&offsets, template->num_vertices,
                  template->num_quads, template->num_hexes,
                  template->num_symmetries, template->num_boundary_vertices,
                  template->flags);

  void *new_data;
  if (posix_memalign(&new_data, ALIGNMENT, n * (size_t)offsets.size) != 0)
    return OUT_OF_MEMORY;

  size_t old_capacity = pool->capacity;

  memcpy(new_data, pool->data, pool->element_size * old_capacity);
  free(pool->data);
  pool->data = new_data;

  pool->capacity = n;

  template = solver_pool_get(pool, 0);

  for (uint32_t i = 0; i < n; i++) {
    solver *solver = solver_pool_get(pool, i);

    solver->num_vertices          = template->num_vertices;
    solver->num_quads             = template->num_quads;
    solver->num_hexes             = template->num_hexes;
    solver->num_symmetries        = template->num_symmetries;
    solver->num_boundary_vertices = template->num_boundary_vertices;

    solver->cb = template->cb;
    solver->user_data = template->user_data;

    if (i >= old_capacity) {
      solver->no_goods = NULL;
      solver->no_goods_size     = 0;
      solver->no_goods_capacity = 0;

      solver->ub_data = NULL;
    }

    apply_offsets(solver, &offsets);
  }

  return SUCCESS;
}

void solver_pool_release(solver_pool *pool) {
  for (uint32_t i = 0; i < pool->capacity; i++) {
    solver *solver = solver_pool_get(pool, i);
    free(solver->no_goods);
  }
  free(pool->data);
}

static inline uint32_t align(uint32_t offset) {
  if (offset % ALIGNMENT != 0) offset += ALIGNMENT - (offset % ALIGNMENT);
  return offset;
}

static inline uint32_t reserve_aligned(uint32_t *offset, uint32_t size) {
  uint32_t result = *offset = align(*offset);
  *offset += size;
  return result;
}

static
void compute_offsets(pool_offsets *offsets,
                     uint32_t num_vertices, uint32_t num_quads,
                     uint32_t num_hexes, uint32_t num_symmetries,
                     uint32_t num_boundary_vertices, uint8_t flags) {
  offsets->size = sizeof(solver);

  offsets->quads = reserve_aligned(
    &offsets->size, 4 * sizeof(vertex_index) * num_quads);
  offsets->hexes = reserve_aligned(
    &offsets->size, 8 * sizeof(vertex_index) * num_hexes);

  offsets->hex_adjacency = reserve_aligned(
    &offsets->size, 6 * sizeof(hex_index) * num_hexes);
  offsets->hex_facets = reserve_aligned(
    &offsets->size, 6 * sizeof(quad_index) * num_hexes);

  if (flags & SOLVER_SHELLABLE_ONLY)
    offsets->quad_adjacency = reserve_aligned(
      &offsets->size, 4 * sizeof(quad_edge) * num_quads);
  else
    offsets->quad_adjacency = offsets->size;

  offsets->quad_queue = reserve_aligned(
    &offsets->size, sizeof(quad_index) * num_quads);
  offsets->num_occurrences = reserve_aligned(
    &offsets->size, sizeof(uint32_t) * num_quads);

  offsets->quad_descriptors = reserve_aligned(
    &offsets->size, sizeof(quad_descriptor) *
    num_vertices * num_vertices);

  offsets->allowed_neighbors = reserve_aligned(
    &offsets->size, sizeof(vertex_set) * num_vertices);

  offsets->known_neighbors = reserve_aligned(
    &offsets->size, sizeof(vertex_set) * num_vertices);

  offsets->known_diagonals = reserve_aligned(
    &offsets->size, sizeof(vertex_set) * num_vertices);

  offsets->known_quad_diagonals = reserve_aligned(
    &offsets->size, sizeof(vertex_set) * num_vertices);

  offsets->symmetries = reserve_aligned(
    &offsets->size, sizeof(vertex_index) *
    num_boundary_vertices * num_symmetries);

  if (flags & SOLVER_SHELLABLE_ONLY)
    offsets->symmetry_data = reserve_aligned(
      &offsets->size, solver_incremental_symmetry_data_size(
        num_vertices, num_hexes, num_symmetries));
  else
    offsets->symmetry_data = offsets->size;

  offsets->size = align(offsets->size);
}

static
void apply_offsets(solver *solver, const pool_offsets *offsets) {
  char *base = (char*)solver;

  solver->quads = (vertex_index*)(base + offsets->quads);
  solver->hexes = (vertex_index*)(base + offsets->hexes);

  solver->hex_adjacency  = (hex_index*)(base + offsets->hex_adjacency);
  solver->hex_facets     = (quad_index*)(base + offsets->hex_facets);
  solver->quad_adjacency = (quad_edge*)(base + offsets->quad_adjacency);

  solver->quad_queue      = (quad_index*)(base + offsets->quad_queue);
  solver->num_occurrences = (uint32_t*)(base + offsets->num_occurrences);

  solver->quad_descriptors = (quad_descriptor*)(
    base + offsets->quad_descriptors);

  solver->allowed_neighbors = (vertex_set*)(base + offsets->allowed_neighbors);
  solver->known_neighbors   = (vertex_set*)(base + offsets->known_neighbors);
  solver->known_diagonals   = (vertex_set*)(base + offsets->known_diagonals);
  solver->known_quad_diagonals = (vertex_set*)(base + offsets->known_quad_diagonals);

  solver->boundary_group = (vertex_index*)(base + offsets->symmetries);
  solver->symmetry_data = (vertex_index*)(base + offsets->symmetry_data);
}

error_code read_symmetries(const char *path, vertex_index **vertices,
                           vertex_index *num_boundary_vertices,
                           size_t *num_symmetries) {
  error_code status = SUCCESS;

  vertex_index current_line[VERTEX_SET_MAX];
  size_t size = 0;
  size_t capacity = 0;

  FILE *file = fopen(path, "r");
  if (!file) return IO_ERROR;

  *vertices = NULL;

  char buffer[1024];
  while (fgets(buffer, sizeof(buffer), file)) {
    size_t len = strlen(buffer);
    if (len == 0) continue;
    if (buffer[len - 1] != '\n') {
      status = IO_ERROR;
      break;
    }

    size_t v_i = 0;
    size_t i = 0;
    while (buffer[i] && (isspace(buffer[i]) || buffer[i] == ',')) i++;

    while (buffer[i]) {
      if (v_i == VERTEX_SET_MAX) break;

      char *end = NULL;
      current_line[v_i++] = strtol(buffer + i, &end, 10);
      if (end == NULL) break;

      i = end - buffer;

      while (buffer[i] && (isspace(buffer[i]) || buffer[i] == ',')) i++;
    }

    if (size == 0)
      *num_boundary_vertices = v_i;

    if (buffer[i]) break;
    if (v_i != *num_boundary_vertices) break;

    if (size + *num_boundary_vertices > capacity) {
      size_t new_capacity = 3 * capacity / 2;
      if (new_capacity < size + *num_boundary_vertices)
        new_capacity = size + *num_boundary_vertices;

      void *buffer = realloc(*vertices, new_capacity * sizeof(**vertices));
      if (!buffer)
        break;

      *vertices = buffer;
    }

    memcpy(*vertices + size, current_line,
           *num_boundary_vertices * sizeof(**vertices));
    size += *num_boundary_vertices;
  }

  *num_symmetries = size / (*num_boundary_vertices);

  if (!feof(file))
    status = IO_ERROR;

  if (status != SUCCESS)
    free(*vertices);

  fclose(file);
  return status;
}
